#include "center_detection/cover_detector.hpp"
#include <algorithm>
#include <cmath>
#include <geometry_msgs/msg/point_stamped.hpp>

namespace center_detection{
    void CoverDetector::setParameters(rclcpp::Node* node) {

        node->get_parameter("cover.handle_search_start_z", params_.handle_search_start_z);
        node->get_parameter("cover.handle_search_end_z", params_.handle_search_end_z);
        node->get_parameter("cover.handle_search_step", params_.handle_search_step);
        node->get_parameter("cover.handle_height_range", params_.handle_height_range);
        node->get_parameter("cover.handle_min_points", params_.handle_min_points);
        node->get_parameter("cover.handle_z_offset", params_.handle_z_offset);

        node->get_parameter("cover.circle_search_start_z", params_.circle_search_start_z);
        node->get_parameter("cover.circle_search_end_z", params_.circle_search_end_z);
        node->get_parameter("cover.circle_search_step", params_.circle_search_step);
        node->get_parameter("cover.circle_height_range", params_.circle_height_range);
        node->get_parameter("cover.circle_min_points", params_.circle_min_points);
        node->get_parameter("cover.circle_min_points_final", params_.circle_min_points_final);

        node->get_parameter("cover.quantization_step", params_.quantization_step);
        node->get_parameter("cover.percentile_low", params_.percentile_low);
        node->get_parameter("cover.percentile_high", params_.percentile_high);
        node->get_parameter("cover.center_merge_threshold", params_.center_merge_threshold);

    }

    bool CoverDetector::detectCenter(PointCloud cloud, geometry_msgs::msg::PointStamped& center_point) {

        if(cloud->empty()) {
            return false;
        }

        //1. 检测把手区域
        float detected_z = NAN;
        auto handle_cloud = detectHandleRegion(cloud, detected_z);
        if(std::isnan(detected_z)) {
            return false;
        } 

        //2. 投影到XY平面
        auto xy_cloud = projectToXY(handle_cloud);

        //3. 检测把手中心
        float x_center, y_center;
        if(!detectHandleCenterX(xy_cloud, x_center, y_center)) {
            return false;
        }

        //4. 检测井盖圆形区域中心
        float circle_x, circle_y;
        if(!detectCircleCenter(cloud, circle_x,circle_y)) {
            return false;
        }

        //5. 合并两个中心点
        float final_x, final_y;
        mergeCenters(x_center, y_center, circle_x,circle_y, final_x, final_y);

        //6. 设置中心点
        center_point.point.x = final_x;
        center_point.point.y = final_y;
        center_point.point.z = detected_z;
        center_point.header.frame_id = "map";

        ////////
        RCLCPP_INFO(rclcpp::get_logger("********manhole_detector********"), 
                "Handle center final position: (%.3f, %.3f, %.3f)", 
                final_x, final_y, detected_z);

        return true;

    }


    CoverDetector::PointCloud CoverDetector::detectHandleRegion(const PointCloud& cloud,
                                                                    float& detected_z) {
                                    
        auto handle_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
        detected_z = NAN;
        
        for(float current_z = params_.handle_search_start_z;
            current_z > params_.handle_search_end_z;
            current_z -= params_.handle_search_step) {

                handle_cloud->clear();

                //提取当前高度切片
                for(const auto& point : *cloud) {
                    if(point.z >= current_z && point.z <= current_z + params_.handle_height_range) {
                        handle_cloud->push_back(point);
                    }
                }

                //检查点云密度
                if(static_cast<int>(handle_cloud->size()) > params_.handle_min_points) {
                    detected_z = current_z + params_.handle_z_offset;
                    break;
                }
            }

            return handle_cloud;

        }

    pcl::PointCloud<pcl::PointXYZ>::Ptr CoverDetector::projectToXY(const PointCloud& cloud) {
            
        auto xy_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

        for(const auto& point : *cloud) {
            pcl::PointXYZ xy_point;
            xy_point.x = point.x;
            xy_point.y = point.y;
            xy_point.z = 0; //投影到z=0平面
            xy_cloud->push_back(xy_point);
        }

        return xy_cloud;
    }


    bool CoverDetector::detectHandleCenterX(const pcl::PointCloud<pcl::PointXYZ>::Ptr& xy_cloud,
                                            float& x_center, float& y_center) {
            
        //计算点云y轴最大最小值和中心点y坐标值
        std::vector<float> y_values;
        for(const auto& point : *xy_cloud) {
            y_values.push_back(point.y);
        }

        if(y_values.empty()) {
            return false;
        }

        std::sort(y_values.begin(), y_values.end());

        //使用分位数作为有效范围（避免离群点）
        size_t low_index = static_cast<size_t>(y_values.size() * params_.percentile_low);
        size_t high_index = static_cast<size_t>(y_values.size() * params_.percentile_high);
        float min_y = y_values[low_index];
        float max_y = y_values[high_index];
        y_center = (min_y + max_y) / 2.0f;


        //统计点云x轴方向上坐标出现次数
        std::unordered_map<int, int> x_count;
        for(const auto& point : *xy_cloud) {
            if(point.y >= min_y && point.y <= max_y) {
                int x_key = static_cast<int>(std::round(point.x / params_.quantization_step));
                x_count[x_key]++;
            }
        }

        //找到出现次数最多的x坐标
        if(x_count.empty()) {
            return false;
        }

        auto max_x_entry = std::max_element(x_count.begin(), x_count.end(),
                                            [](const auto& a, const auto& b) {
                                                return a.second < b.second;
                                            });
        x_center = max_x_entry->first * params_.quantization_step;

        return true;
        
    }

    bool CoverDetector::detectCircleCenter(const PointCloud& cloud,
                                                float& circle_x, float& circle_y) {
        auto circle_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();

        //从指定高度开始向下搜索
        for(float current_z = params_.circle_search_start_z;
            current_z > params_.circle_search_end_z;
            current_z -= params_.circle_search_step) {

            circle_cloud->clear();

            //提取当前高度切片
            for(const auto& point : *cloud) {
                if(point.z >= current_z &&
                    point.z <= current_z + params_.circle_height_range) {
                    circle_cloud->push_back(point);
                }
            }

            //检查点云密度
            if(static_cast<int>(circle_cloud->size()) > params_.circle_min_points) {
                break;
            }

        }

        if(static_cast<int>(circle_cloud->size()) < params_.circle_min_points_final) {
            return false;
        }

        //将点云投影到XY平面
        Eigen::MatrixXf points(circle_cloud->size(), 2);
        for(size_t i = 0; i < circle_cloud->size(); ++i) {
            points(i, 0) = circle_cloud->points[i].x;
            points(i, 1) = circle_cloud->points[i].y;
        }

        //构建线性方程组
        Eigen::VectorXf x = points.col(0);
        Eigen::VectorXf y = points.col(1);
        Eigen::VectorXf x2 = x.array().square();
        Eigen::VectorXf y2 = y.array().square();

        Eigen::MatrixXf A(circle_cloud->size(), 3);
        A.col(0) = 2 * x;
        A.col(1) = 2 * y;
        A.col(2) = Eigen::VectorXf::Ones(circle_cloud->size());

        Eigen::VectorXf b = x2 + y2;

        //求解线性方程组
        Eigen::Vector3f circle_params = A.colPivHouseholderQr().solve(b);
        circle_x = circle_params[0];
        circle_y = circle_params[1];

        return true;
    }

    void CoverDetector::mergeCenters(float handle_x, float handle_y,
                                       float circle_x, float circle_y, 
                                       float& final_x, float& final_y) {
        float dist = std::hypot(handle_x - circle_x, handle_y - circle_y);
        if(dist < params_.center_merge_threshold) {
            final_x = handle_x;
            final_y = handle_y;
            ////////
            RCLCPP_INFO(rclcpp::get_logger("******manhole_detector********"), 
                    "Using handle center: (%.3f, %.3f)", handle_x, handle_y);
        }
        else {
            final_x = circle_x;
            final_y = circle_y;
            ///////
            RCLCPP_INFO(rclcpp::get_logger("******manhole_detector********"), 
                    "Using circle center: (%.3f, %.3f)", circle_x, circle_y);
        }
    }

}
